import random
import motionplanning
import cspace

class IterativeSubproblem:
    def plan_more(self):
        pass

    def done(self):
        """Return true if the subproblem is successfully completed or
        has failed."""
        return False

    def failed(self):
        """Return true if the subproblem has failed"""
        return False

    def estimate_cost(self):
        """In future versions this might be used to pick better subproblems"""
        return None

    def solution(self):
        """Used to return the solution"""
        return None


class FakeIterativeSubproblem(IterativeSubproblem):
    def __init__(self,a=0,b=100):
        self.timeLeft = random.uniform(a,b)

    def plan_more(self):
        self.timeLeft -= 1.0

    def done(self):
        return self.timeLeft <= 0

    def failed(self):
        return False

    def solution(self):
        return None


class MotionPlanningSubproblem(IterativeSubproblem):
    def __init__(self,space,start,goal,options=None):
        self.space = space
        self.start = start
        self.goal = goal
        self.path = None
        self.infeasible = False
        if options:
            cspace.MotionPlan.setOptions(**options)
        self.plan = cspace.MotionPlan(space)
        if not space.feasible(start) or not space.feasible(goal):
            self.infeasible = True
        else:
            self.plan.setEndpoints(start,goal)

        
    def plan_more(self):
        self.plan.planMore(1)

    def done(self):
        if self.infeasible: return True
        if self.path==None:
            self.path = self.plan.getPath()
        return self.path!=None

    def failed(self):
        #except for when endpoints are infeasible, we can't ever tell
        #if the plan has failed
        return self.infeasible

    def solution(self):
        if self.path==None:
            self.path = self.plan.getPath()
        return self.path
